.TITLE EULER

;EULER  - PROGRAMS FOR CONVERTING TRANSFORMS ↔ EULER ANGLES

;DEFINED GLOBALS

.GLOBL  EULER,UNEUL

;REQUIRED GLOBALS

.GLOBL  SQRT,SNCOS,ATAN2

;DEFINITIONS

R0	=%0
R1	=%1
R2	=%2
R3	=%3
R4	=%4
R5	=%5
SP	=%6
PC	=%7

;INDICES TO TRANFORM ELEMENT, STORED BY COLUMNS

T21	=2
T31	=4
T12	=6
T22	=10
T32	=12
T13	=14
T23	=16
T33	=20
T14	=22
T24	=24
T34	=26
;"EULER" COMPUTES THE EULER ANGLES DESCRIBING AN ARM POSITION GIVEN
;A TRANSFORM IN TABLE COORDINATES.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;LOAD ADDR. OF TRANSFORM "T"
;		MOV	#E,R1		;LOAD ADDR. OF ARRAY TO 
;					;  RETURN THE EULER ANGLES
;		JSR	PC,EULER	;CALLED USING THE PC
;
;THE TRANSFORM "T" MUST BE STORED BY COLUMNS AND THE ARRAY "E" MUST
;BE AT LEAST 6 WORDS LONG.  ALL NUMBERS SHOULD BE IN SCALED
;INTEGER FORMAT.

;REGISTERS USED:
;
;	R0, R1 PASS ARGUMENTS AND ARE ALTERED BY "EULER"

;START OF "EULER"

EULER:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	R0,R4		;SAVE POINTERS
	MOV	R1,R5
      	MOV	T14(R4),(R5)+	;COPY X LOCATION INTO E(1)
	MOV	T24(R4),(R5)+	;COPY Y LOCATION INTO E(2)
	MOV	T34(R4),(R5)+	;COPY Z LOCATION INTO E(3)
	MOV	T13(R4),R0	;COMPUTE CO2←SQRT(T[1,3]↑2+T[2,3]↑2)
	MUL	R0,R0
	MOV	T23(R4),R2
	MUL	R2,R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	JSR	PC,SQRT
	MOV	R0,R2   	;SAVE CO2 FOR LATER
	MOV	R0,R1    	;COMPUTE ATAN(-T[3,3]/CO2)
	MOV	T33(R4),R0
	NEG	R0
	JSR	PC,ATAN2
	MOV	R0,2(R5)	;THIS IS E(5)
	CMP	#243,R2    	;CHECK IF CO2 ALMOST ZERO
	BLE	CO2BIG		;IF CO2<0.01
	MOV	T22(R4),R0	;THEN E(4)←ATAN(T[2,2]/T[1,2])
	MOV	T12(R4),R1
	JSR	PC,ATAN2
	MOV	R0,(R5)
	CLR	4(R5)		;AND E(6)←0
	BR	EULDNE		;RETURN
CO2BIG:	MOV	T13(R4),R0 	;ELSE E[4]←ATAN(T[1,3]/-T[2,3])
	MOV	T23(R4),R1 
	NEG 	R1
	JSR	PC,ATAN2
	MOV	R0,(R5)
	MOV	T32(R4),R0 	;AND E[6]←ATAN(T[3,2]/-T[3,1])
	MOV	T31(R4),R1 
	NEG 	R1 
	JSR	PC,ATAN2
	MOV	R0,4(R5)
EULDNE:	MOV	(SP)+,R5	;RESTORE REGISTERS
	MOV	(SP)+,R4
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC


;END OF "EULER"
;"UNEUL" COMPUTES A TRANSFORM MATRIX REPRESENTING HAND POSITION AND 
;ORIENTATION IN TABLE COORDINATES GIVEN THE EULER ANGLES OF THE 
;LOCATION.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#T,R0		;ADDR. OF TRANSFORM "T"
;		MOV	#E,R1		;ADDR. OF EULER ANGLE ARRAY
;		JSR	PC,UNEUL	;CALLED USING THE PC
;
;THE TRANSFORM "T" WILL BE STORED BY COLUMNS AND THE ARRAY "E" MUST
;CONTAIN THE 6 EULER ANGLES.  ALL NUMBERS SHOULD BE IN SCALED 
;INTEGER FORMAT.

;REGISTERS USED:
;
;	R0, R1 PASS ARGUMENTS AND ARE ALTERED BY "UNEUL"

;RELATIVE STACK POINTERS FOR SINE/COSINES

SI1	=12	;SINE/COSINE E[4]
CO1	=10
SI2	=6	;SINE/COSINE E[5]
CO2	=4
SI3	=2	;SINE/COSINE E[6]
CO3	=0

UNEUL:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	R0,R4		;SAVE POINTERS
	MOV	R1,R5
      	MOV	(R5)+,T14(R4)	;COPY X LOCATION INTO T[1,4]
	MOV	(R5)+,T24(R4)	;COPY Y LOCATION INTO T[2,4]
	MOV	(R5)+,T34(R4)	;COPY Z LOCATION INTO T[3,4]
	MOV	(R5)+,R0 	;COMPUTE THE SINE/COSINE OF E[4]
	JSR	PC,SNCOS
	MOV	R0,-(SP)
	MOV	R1,-(SP)
	MOV	(R5)+,R0 	;COMPUTE THE SINE/COSINE OF E[5]
	JSR	PC,SNCOS
	MOV	R0,-(SP)
	MOV	R1,-(SP)
	MOV	(R5),R0		;COMPUTE THE SINE/COSINE OF E[6]
	JSR	PC,SNCOS
	MOV	R0,-(SP)
	MOV	R1,-(SP)
	MOV	SI1(SP),R2	;T[1,1]←-SI1*SI2*CO3+CO1*SI3
	MUL	SI2(SP),R2
	ASHC	#2,R2
	MOV	R2,-(SP)	;SAVE SI1*SI2
	MUL	R1,R2
	MUL	CO1+2(SP),R0
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0
	MOV	R0,(R4)		;SAVE T[1,1]
	MOV	(SP)+,R0	;T[1,2]←SI1*SI2*SI3+CO1*CO3
	MUL	SI3(SP),R0
	MOV	(SP),R2
	MUL	CO1(SP),R2
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	MOV	R0,T12(R4)	;SAVE T[1,2]
	ASHC	#2,R2      	;T[2,1]←CO1*CO3*SI2+SI1*SI3
	MUL	SI2(SP),R2
	MOV	SI1(SP),R0
	MUL	SI3(SP),R0
	ADD	R3,R1
	ADC	R0
	ADD	R2,R0
	ASHC	#2,R0
	MOV	R0,T21(R4)	;SAVE T[2,1]
	MOV	CO1(SP),R2	;T[2,2]←-CO1*SI2*SI3+SI1*CO3
	MUL	SI2(SP),R2
	ASHC	#2,R2
	MUL	SI3(SP),R2
	MOV	SI1(SP),R0
	MUL	(SP),R0
	SUB	R3,R1
	SBC	R0
	SUB	R2,R0
	ASHC	#2,R0
	MOV	R0,T22(R4)	;SAVE T[2,2]
	MOV 	(SP)+,R0	;T[3,1]←-CO2*CO3
	MUL	CO2-2(SP),R0
	ASHC	#2,R0
	NEG	R0
	MOV	R0,T31(R4)
	MOV 	(SP)+,R0	;T[3,2]←CO2*SI3
	MUL	(SP),R0    
	ASHC	#2,R0
	MOV	R0,T32(R4)
	MOV	(SP),R0		;T[1,3]←SI1*CO2
	MUL	SI1-4(SP),R0
	ASHC	#2,R0   
	MOV	R0,T13(R4)
	MOV	(SP)+,R0	;T[2,3]←-CO1*CO2
	MUL 	CO1-6(SP),R0
	ASHC	#2,R0
	NEG	R0
	MOV	R0,T23(R4)
	NEG	(SP)   		;T[3,3]←-SI2
	MOV	(SP)+,T33(R4)
	CMP	(SP)+,(SP)+	;CLEAR SI1 AND